from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch_ros.parameter_descriptions import ParameterValue
from launch.actions import IncludeLaunchDescription

import os
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    use_sim_time=LaunchConfiguration("use_sim_time",default="false"),

    package_dir = FindPackageShare('robot_xacro_model')
    urdf_path = PathJoinSubstitution([package_dir,'urdf', 'entrance.urdf.xacro'])
    robot_description_content = ParameterValue(Command(['xacro ', urdf_path]), value_type=str)
    gezebo_world_path = get_package_share_directory('robot_xacro_model')+"/world/gazebo.world"

    empty_gazebo_launch_py = IncludeLaunchDescription(
        PathJoinSubstitution([FindPackageShare("robot_xacro_model"),"launch","gazebo","empty_gazebo.launch.py"]),
        # launch_arguments=[('world',gezebo_world_path),('verbose','false')]
    )
    import_urdf2gazebo_launch_py = IncludeLaunchDescription(
        PathJoinSubstitution([FindPackageShare("robot_xacro_model"),"launch","gazebo","import_urdf2gazebo.launch.py"]),
    )
    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        parameters=[{"use_sim_time":use_sim_time,'robot_description': robot_description_content}],
        # arguments=[urdf]
    )

    return LaunchDescription([
        robot_state_publisher,
        empty_gazebo_launch_py,
        import_urdf2gazebo_launch_py,
    ])